package org.hs.phd.odi.tracking.tracker.particlefilter.transition;

import java.util.Random;

import org.hs.phd.common.randomnumbers.GaussianRandomNumberGenerator;
import org.hs.phd.odi.tracking.tracker.particlefilter.core.Particle;
import org.hs.phd.odi.tracking.tracker.particlefilter.core.TransitionModel;
import org.hs.phd.odi.tracking.tracker.particlefilter.state.RotatedRectangleParticleState;

public class RectangleAutoRegressiveDynamicsTransitionModel implements TransitionModel<RotatedRectangleParticleState> {

	/* autoregressive dynamics parameters for transition model */
	private static final float A1 = 2.0F;
	private static final float A2 = -1.0F;
	private static final float B0 = 1.0000F;
	private GaussianRandomNumberGenerator xNoise;
	private GaussianRandomNumberGenerator yNoise;
	private GaussianRandomNumberGenerator sNoise;
	private final int w;
	private final int h;
	private SecondOrderAutoRegressiveTransitionHelper regression;

	/* random number generators for gaussian sampling in transition model */

	public RectangleAutoRegressiveDynamicsTransitionModel(int w, int h) {
		this(new Random(), w, h);
	}

	public RectangleAutoRegressiveDynamicsTransitionModel(Random random, int w, int h) {
		this.w = w;
		this.h = h;
		xNoise = new GaussianRandomNumberGenerator(random, 0, 2F);
		yNoise = new GaussianRandomNumberGenerator(random, 0, 2F);
		sNoise = new GaussianRandomNumberGenerator(random, 0, 0.001F);
		regression = new SecondOrderAutoRegressiveTransitionHelper(A1, A2, B0);
	}

	@Override
	public Particle<RotatedRectangleParticleState> transition(Particle<RotatedRectangleParticleState> p) {

		RotatedRectangleParticleState currentState = p.getCurrentState();
		RotatedRectangleParticleState initialState = p.initialState;
		RotatedRectangleParticleState prevState = p.getPrevState(0);

		/* sample new state using second-order autoregressive dynamics */
		float x = (float) regression.transitionMinMaxTrim(initialState.x, prevState.x, currentState.x, xNoise, 0.0F, (float) w - 1.0F);
		float y = (float) regression.transitionMinMaxTrim(initialState.y, prevState.y, currentState.y, yNoise, 0.0F, (float) h - 1.0F);
		float s = (float) regression.transitionMinMaxTrim(initialState.scale, prevState.scale, currentState.scale, sNoise, 0.2F, 3);

		RotatedRectangleParticleState newState = currentState.createMovedScaledCopy(x, y, s);
		return p.createNextGenerationParticle(newState);
	}

}
